#include "ActionTurn.h"


ActionTurn::ActionTurn(double turnThreshold, double turnAmount) :
	ArAction("Turn") {
	myTurnThreshold = turnThreshold;
	myTurnAmount = turnAmount;
	setNextArgument(ArArg("turn threshold (mm)", &myTurnThreshold,
			"The number of mm away from obstacle to begin turnning."));
	setNextArgument(ArArg("turn amount (deg)", &myTurnAmount,
			"The number of degress to turn if turning."));
	myTurning = 0;
}


void ActionTurn::setRobot(ArRobot *robot) {
	ArAction::setRobot(robot);
	mySonar = robot->findRangeDevice("sonar");
	if (mySonar == NULL) {
		ArLog::log(ArLog::Terse,
				"actionExample: ActionTurn: Warning: I found no sonar, deactivating.");
		deactivate();
	}
}


ArActionDesired *ActionTurn::fire(ArActionDesired currentDesired) {
	double leftRange, rightRange;

	myDesired.reset();

	if (mySonar == NULL) {
		deactivate();
		return NULL;
	}

	leftRange = (mySonar->currentReadingPolar(0, 100)
			- myRobot->getRobotRadius());
	rightRange = (mySonar->currentReadingPolar(-100, 0)
			- myRobot->getRobotRadius());

	if (leftRange > myTurnThreshold && rightRange > myTurnThreshold) {
		myTurning = 0;
		myDesired.setDeltaHeading(0);
	}

	else if (myTurning) {
		myDesired.setDeltaHeading(myTurnAmount * myTurning);
	}

	else if (leftRange < rightRange) {
		myTurning = -1;
		myDesired.setDeltaHeading(myTurnAmount * myTurning);
	}

	else {
		myTurning = 1;
		myDesired.setDeltaHeading(myTurnAmount * myTurning);
	}

	return &myDesired;
}
